# 内置库 
import math
import os
import sys
import importlib
import matplotlib.pyplot as plt
# 第三方库
import numpy as np
from shapely.geometry import LineString, Point, Polygon
# 导入onsite-mine相关模块（库中含有-,因此使用动态importlib动态导入包）
## onsite-mine.common模块
kdtree = importlib.import_module("onsite-unstructured.common.kdtree")


def plot_borderline(observation, linewidth=0.5, ax=None):
    if ax is None:
        ax = plt.gca()
    for i in range(len(observation["hdmaps_info"]["tgsc_map"].borderline)):
        borderpoints = observation["hdmaps_info"]["tgsc_map"].borderline[i]['borderpoints']
        x_values = [point[0] for point in borderpoints]
        y_values = [point[1] for point in borderpoints]
        # ax.plot(x_values, y_values, linewidth=linewidth)
        ax.scatter(x_values, y_values, s=linewidth)


def plot_reference_path(observation, linewidth=0.5, ax=None):
    if ax is None:
        ax = plt.gca()
    for i in range(len(observation["hdmaps_info"]["tgsc_map"].reference_path)):
        waypoints = observation["hdmaps_info"]["tgsc_map"].reference_path[i]['waypoints']
        x_values = [point[0] for point in waypoints]
        y_values = [point[1] for point in waypoints]
        ax.plot(x_values, y_values, linestyle='--', linewidth=linewidth)


def plot_goal(goal_x, goal_y, ax=None):
    if ax is None:
        ax = plt.gca()
    polygon = [[goal_x[0], goal_y[0]], [goal_x[1], goal_y[1]], [goal_x[2], goal_y[2]], [goal_x[3], goal_y[3]]]
    ax.fill(*zip(*polygon), color='y', alpha=0.4, label='goal')

def plot_path_planned(observation, path_planned, start_x, start_y, goal_x, goal_y,
                      x_min, x_max, y_min, y_max,scene_name):
    plot_borderline(observation, linewidth=0.01)
    plot_reference_path(observation, linewidth=0.5)
    plt.scatter(start_x, start_y, marker='o', color='b', s=20, label='start', facecolors='None')
    plot_goal(goal_x, goal_y)
    x_coords = [point[0] for point in path_planned]
    y_coords = [point[1] for point in path_planned]
    plt.scatter(x_coords, y_coords)
    plt.xlim(x_min, x_max)
    plt.ylim(y_min, y_max)
    plt.legend()
    plt.title(scene_name)

def point_in_circle(x, y, x0, y0, r):
    distance = math.sqrt((x - x0)**2 + (y - y0)**2)
    if distance <= r:
        return True
    else:
        return False

# 找出点在数组中的索引
def find_point_index(point, trajectory):
    # 找到所有匹配的行的索引
    indices = np.where((trajectory == point).all(axis=1))[0]
    if indices.size > 0:
        return indices[0]  # 返回第一个匹配的索引
    else:
        return None  # 如果没有匹配的点，返回 None

def has_entered_and_left_area(point, area,inside):
    """
    检查背景车的轨迹是否进入并离开了目标区域。
    :param point: 实时更新的点，格式为 (x, y)
    :param area: 目标区域，多边形对象
    :param inside: 表示是否已经进入目标区域
    :return: 布尔值，表示是否进入并离开了目标区域
    """
    point_obj = Point(point)
    if area.contains(point_obj):
        inside = True
    elif inside and not area.contains(point_obj):
        return True
    return False

def angle_difference(angle1, angle2):
    """
    计算两个角度之间的最小差异。
    :param angle1: 第一个角度（弧度）
    :param angle2: 第二个角度（弧度）
    :return: 最小差异（弧度）
    """
    diff = angle2 - angle1
    while diff < -np.pi:
        diff += 2 * np.pi
    while diff > np.pi:
        diff -= 2 * np.pi
    return abs(diff)

def is_same_direction(yaw_ego_list, yaw_background_list, threshold=np.pi / 6, min_count=5):
    """
    判断自车与背景车是否同向。
    :param yaw_ego_list: 自车的yaw值列表（弧度）
    :param yaw_background_list: 背景车的yaw值列表（弧度）
    :param threshold: 角度差异阈值（弧度），默认为30度（pi/6）
    :param min_count: 判定为同向的最小数量
    :return: 布尔值，表示是否同向
    """
    count = 0
    for yaw_ego in yaw_ego_list:
        for yaw_background in yaw_background_list:
            if angle_difference(yaw_ego, yaw_background) < threshold:
                count += 1
    return count >= min_count

def truncate_path_after_area(path, area, extra_points=10):
    """
    从路径中移除进入目标区域后的点，并在进入目标区域后再添加指定数量的点。
    :param path: 路径点，格式为 Nx5 的数组
    :param area: 目标区域，多边形对象
    :param extra_points: 进入目标区域后要继续添加的点数
    :return: 截断后的路径
    """
    truncated_path = []
    inside_area = False
    extra_count = 0

    for point in path:
        point_obj = Point(point[:2])  # 只取前两个值作为点的坐标
        truncated_path.append(point)

        if area.contains(point_obj):
            inside_area = True

        if inside_area:
            extra_count += 1
            if extra_count > extra_points:
                break

    return np.array(truncated_path)

def paths_intersect(path1, path2):
    """
    判断两条路径是否相交。
    :param path1: 第一条路径点，格式为 Nx2 的数组
    :param path2: 第二条路径点，格式为 Mx2 的数组
    :return: 布尔值，表示两条路径是否相交
    """
    if len(path1) < 2 or len(path2) < 2:
        # 如果路径中的任何一个点数少于 2，则无法创建 LineString
        return False
    line1 = LineString(path1)
    line2 = LineString(path2)
    return line1.intersects(line2)

class Point_t:
    """2维k-d tree的基础数据结构,KDNode."""

    def __init__(self, x: float = None, y: float = None, ind: int = None):
        """_summary_

        Args:
            x (float, optional): 笛卡尔坐标系x. Defaults to None.
            y (float, optional): 笛卡尔坐标系y. Defaults to None.
            ind (int, optional): 在data中的结果索引inddex. Defaults to None.
        """
        self.x = x
        self.y = y
        self.ind = ind


class Item(object):
    def __init__(self, x, y, data):
        self.coords = (x, y)
        self.data = data

    def __len__(self):
        return len(self.coords)

    def __getitem__(self, i):
        return self.coords[i]

    def __repr__(self):
        return "Item({},{},{})".format(self.coords[0], self.coords[1], self.data)

class MotionController:
    """运动控制模块.
    功能设计:跟踪规划模块输入的路径曲线,速度曲线;
    """

    def __init__(self):
        self._min_prev_dist = 10  # 最小预瞄距离
        self._prev_k = 0.1  # 预瞄距离速度系数
        self.L = 5.6  # 轴距
        self._is_create_kdtree = False
        self._projection_vehicle_S = list()  # 存储投影车
        self._last_acc = -999
        self.traj_true_dict = {}
        self.intersect_all = True
        self.start = True
        self.x_start = None
        self.y_start = None
        self.inside = False
        self.time_car = 5.5

    def process(self, vehicle_info, path_waypoints, traj_future, observation, traj_true,replan=False):
        """控制主函数
        输入:_a_max自车最大加速度,_vel自车当前车速,_vel0自车期望车速,delta加速度指数,
        _dv自车与前车速度差,_s自车与前车车距,_b舒适减速度,_t安全时距,_s0最小车距
        """
        #################更新参数#################
        self._ego_v = vehicle_info["ego"]["v_mps"]
        self._ego_x = vehicle_info["ego"]["x"]
        self._ego_y = vehicle_info["ego"]["y"]
        self._ego_yaw = vehicle_info["ego"]["yaw_rad"]
        self._ego_shape = vehicle_info["ego"]["shape"]
        self._vehicle_info = vehicle_info
        self._projection_vehicle_S.clear()
        area = Polygon(list(zip(observation['test_setting']["goal"]["x"],observation['test_setting']["goal"]["y"])))
        path = path_waypoints
        
        path_waypoints = truncate_path_after_area(path_waypoints,area)
        # scene_name = observation['test_setting']['scenario_name']
        # goal_x = observation['test_setting']["goal"]["x"]
        # goal_y = observation['test_setting']["goal"]["y"]
        # start_x = observation['vehicle_info']["ego"]["x"]
        # start_y = observation['vehicle_info']["ego"]["y"]
        #
        # utm_local_range = observation['hdmaps_info']['image_mask'].bitmap_local_info['utm_local_range']
        # x_min, y_min, x_max, y_max = utm_local_range[0], utm_local_range[1], utm_local_range[2], \
        #                              utm_local_range[3]
        # x_margin, y_margin = 20, 20
        # x_min, x_max, y_min, y_max = x_min - x_margin, x_max + x_margin, y_min - y_margin, y_max + y_margin
        # plt.figure(figsize=(6.2, 5), dpi=400)
        # plot_path_planned(observation, path_waypoints, start_x, start_y, goal_x, goal_y,
        #                  x_min, x_max, y_min, y_max,scene_name)
        # plt.show()

        #################只创建一次kdtree#################
        if self._is_create_kdtree == False:
            path_tuple = list()
            i = 0  # 记录路径点下标
            for waypoint in path_waypoints:
                point = Item(waypoint[0], waypoint[1], str(i))  #! (x,y,index) 用于构建KD-tree
                path_tuple.append(point)
                i += 1
            self._tree = kdtree.create(path_tuple)

        #################匹配与车辆最近的路径点#################
        nearest_tree_node = self._tree.search_nn([self._ego_x, self._ego_y])
        self._nearest_pt_id = int(nearest_tree_node[0].data.data)  # 最近路径点的下标
        self._nearest_pt = path_waypoints[self._nearest_pt_id]
        path_waypoints_ego = np.array(path_waypoints)[self._nearest_pt_id:,:2]
        # yaw_ego = np.array(path_waypoints)[-10:,2]

        for id_veh in traj_true.keys():
            traj_list = []
            if id_veh in self.traj_true_dict or (id_veh in self.traj_true_dict and self.traj_true_dict[id_veh] == []):
                break
            for time, traj in traj_true[id_veh].items():
                if time == 'shape':
                    continue
                traj_list.append([traj['x'],traj['y']])
            self.traj_true_dict[id_veh] = traj_list
            # direction = is_same_direction(yaw_ego_list=yaw_ego,yaw_background_list=np.array(self.traj_true_dict[id_veh])[-10:,2])
            # area = Polygon(list(zip(observation['test_setting']["goal"]["x"],observation['test_setting']["goal"]["y"])))
            # self.traj_true_dict[id_veh]['arrive_car'] = has_entered_and_left_area((vehicle_info[id_veh]["x"],vehicle_info[id_veh]["y"]),area,self.inside)
        for id_veh in observation["vehicle_info"].keys():
            if id_veh == "ego":
                continue
            veh_x = observation["vehicle_info"][id_veh]["x"]
            veh_y = observation["vehicle_info"][id_veh]["y"]
            veh_point = np.array([veh_x, veh_y])
            if observation["test_setting"]["scenario_type"] == "mission":
                id_t = id_veh+1
            else:
                id_t = id_veh
            index = find_point_index(veh_point, np.array(self.traj_true_dict[id_t]))
            if index == None:
                continue
            else:
                self.traj_true_dict[id_t] = self.traj_true_dict[id_t][index:]
            if len(self.traj_true_dict[id_t]) == 1:
                self.traj_true_dict[id_t] = []

            # 检测背景车轨迹是否与主车轨迹有交集
        intersect_list = []
        for id_veh, trajectory in self.traj_true_dict.items():
            trajectory_2d = np.array(trajectory)  # 提取x和y值
            intersect = paths_intersect(path_waypoints_ego,trajectory_2d)
            intersect_list.append(intersect)

        if True not in intersect_list:
            self.intersect_all = False

        if self.start:
            self.x_start = vehicle_info["ego"]["x"]
            self.y_start = vehicle_info["ego"]["y"]
            self.start = False
        point_in = point_in_circle(vehicle_info["ego"]["x"],vehicle_info["ego"]["y"],self.x_start,self.y_start,0.1)
        #################计算预瞄距离#################
        prev_dist = self._min_prev_dist + self._prev_k * self._ego_v

        #################寻找预瞄点#################
        prev_pt = Point_t(0, 0, -10000)
        if len(path_waypoints) == 0:
            print("###log### PATH EMPTY!!!")
        for i in range(-1, -len(path_waypoints) - 1, -1):
            temp_dist = math.hypot(path_waypoints[i][0] - self._ego_x, path_waypoints[i][1] - self._ego_y)
            if temp_dist < prev_dist:
                prev_pt.x = path_waypoints[i][0]
                prev_pt.y = path_waypoints[i][1]
                ind = i
                prev_pt.ind = 0
                break
        if prev_pt.ind == -10000:
            print("CAN NOT FIND PREVIEW POINT!!!")
        else:  # 如果预瞄点在路径的第一个点,则将其设置为第一个点
            dx1 = path[ind][0] - self._ego_x
            dy1 = path[ind][1] - self._ego_y
            yaw0 = math.atan2(dy1, dx1)
            scene_name = observation['test_setting']['scenario_name']
            if 'mission' in scene_name and abs(yaw0- self._ego_yaw) > 0.09:
                if abs(yaw0- self._ego_yaw) > 0.15:
                    taget_v = 4.5
                prev_dist = 7
                for i in range(ind, -len(path_waypoints) - 1, -1):
                    temp_dist = math.hypot(path_waypoints[i][0] - self._ego_x, path_waypoints[i][1] - self._ego_y)
                    if temp_dist < prev_dist:
                        prev_pt.x = path_waypoints[i][0]
                        prev_pt.y = path_waypoints[i][1]
                        prev_pt.ind = 0
                        break
        #################纯跟踪输出前轮转角#################
        delta = self._purePursuit(prev_pt)

        #################判断前方是否有车#################
        front_vehicle, back_vehicle = self._findFrontVehicle(vehicle_info, path_waypoints, traj_future, observation)

        #################IDM输出加速度#################
        # if back_vehicle != None:
        #     dv = self._ego_v - back_vehicle["v_mps"]
        #     s = (
        #             math.hypot(self._ego_x - back_vehicle["x"], self._ego_y - back_vehicle["y"])
        #             - math.hypot(self._ego_shape["width"], self._ego_shape["length"]) / 2
        #             - math.hypot(back_vehicle["shape"]["width"], back_vehicle["shape"]["length"]) / 2
        #     )
        #     acc = self._IDM(2.2, self._ego_v, 10, 100.0, dv, s, 1.0, 3.5, 30)
        # else:
        #     acc = self._IDM(2.2, self._ego_v, 10, 100.0, 0.0, 9999.0, 1.0, 3.5, 30)
        if point_in:
            acc = self._IDM(0.1, self._ego_v, 9.3, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
        else:
            scene_name = observation['test_setting']['scenario_name']
            if front_vehicle != None:
                dv = self._ego_v - front_vehicle["v_mps"]
                s = (
                    math.hypot(self._ego_x - front_vehicle["x"], self._ego_y - front_vehicle["y"])
                    - math.hypot(self._ego_shape["width"], self._ego_shape["length"]) / 2
                    - math.hypot(front_vehicle["shape"]["width"], front_vehicle["shape"]["length"]) / 2
                )
                acc = self._IDM(1.4, self._ego_v, 9.3, 100.0, dv, s, 1.0, 4.0, 28)
            else:
                if self.intersect_all == False:
                    location = observation['hdmaps_info']['image_mask'].location
                    if location == 'guangdong_dapai':
                        acc = self._IDM(2, self._ego_v, 12, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                        if scene_name in ['dapai_intersection_1_2_17']:
                            acc = self._IDM(1.5, self._ego_v, 10, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    elif location == 'jiangxi_jiangtong':
                        acc = self._IDM(1.5, self._ego_v, 10, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    else:
                        # print("CAN NOT FIND LOCATION!!!")
                        acc = self._IDM(2, self._ego_v, 10, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    if scene_name in ['jiangtong_intersection_6_3_1',   # 转弯要慢一点
                                      'jiangtong_intersection_6_3_6']:
                        acc = self._IDM(1.5, self._ego_v, 7, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)

                    if scene_name in ['jiangtong_intersection_1_2_6'] and observation['test_setting']['t'] > 8:
                        acc = self._IDM(1.5, self._ego_v, 12, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    if scene_name in ['jiangtong_intersection_1_3_10'] and observation['test_setting']['t'] > 43:
                        acc = self._IDM(1.5, self._ego_v, 12, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    if scene_name in ['jiangtong_intersection_5_3_2'] and observation['test_setting']['t'] > 17:
                        acc = self._IDM(1.5, self._ego_v, 12, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    if scene_name in ['dapai_intersection_1_2_18'] and observation['test_setting']['t'] > 10:
                        acc = self._IDM(3, self._ego_v, 13, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    if scene_name in ['dapai_intersection_1_3_17'] and observation['test_setting']['t'] > 8:
                        acc = self._IDM(1.5, self._ego_v, 6, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    if scene_name in ['dapai_intersection_1_2_4','dapai_intersection_1_2_7','dapai_intersection_1_2_8','dapai_intersection_1_2_9','dapai_intersection_1_3_10','dapai_intersection_1_3_12','dapai_intersection_1_3_14','dapai_intersection_1_3_16','dapai_intersection_1_3_5','dapai_intersection_1_3_8','dapai_intersection_1_2_12','dapai_intersection_1_4_2','dapai_intersection_1_4_6','dapai_intersection_1_2_13','dapai_intersection_1_4_8','dapai_intersection_2_2_5','dapai_intersection_1_2_14','jiangtong_intersection_1_2_10', 'jiangtong_intersection_1_3_11','jiangtong_intersection_1_3_12','jiangtong_intersection_1_3_4', 'jiangtong_intersection_1_3_5','jiangtong_intersection_1_3_6','jiangtong_intersection_1_3_7', 'jiangtong_intersection_1_3_8','jiangtong_intersection_1_4_1','jiangtong_intersection_1_4_2', 'jiangtong_intersection_1_4_3','jiangtong_intersection_1_4_4','jiangtong_intersection_1_5_1', 'jiangtong_intersection_1_5_2','jiangtong_intersection_1_5_3','jiangtong_intersection_1_5_5', 'jiangtong_intersection_1_7_5','jiangtong_intersection_2_8_4','jiangtong_intersection_5_2_1', 'jiangtong_intersection_5_4_2','jiangtong_intersection_6_2_1','jiangtong_intersection_6_2_5', 'jiangtong_intersection_6_3_4','jiangtong_intersection_6_3_5','jiangtong_intersection_1_2_8', 'jiangtong_intersection_1_2_9','jiangtong_intersection_1_3_1']:
                        acc = self._IDM(5, self._ego_v, 15.5, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    # if replan == True:  # 速度策略激进
                    #     if location == 'guangdong_dapai':
                    #         acc = self._IDM(2, self._ego_v, 12, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                    #     elif location == 'jiangxi_jiangtong':
                    #         acc = self._IDM(1.5, self._ego_v, 12, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
                else:
                    acc = self._IDM(1.7, self._ego_v, 8.0, 100.0, 0.0, 9999.0, 1.0, 4.0, 28)
        if self._last_acc != -999:
            if acc - self._last_acc > 1.0:
                acc = self._last_acc + 1
            elif acc - self._last_acc < -1.0:
                acc = self._last_acc - 1
        self._last_acc = acc
        return (acc, delta), self.traj_true_dict

    def _IDM(self, a_max, vel, vel0, delta, dv, s, b, t, s0):
        """智能驾驶模型,计算加速度.[纵向跟踪控制模块]
        输入:_a_max自车最大加速度,_vel自车当前车速,_vel0自车期望车速,delta加速度指数,
        _dv自车与前车速度差,_s自车与前车车距,_b舒适减速度,_t安全时距,_s0最小车距
        """
        expected_s = vel * t + vel * dv / (2 * math.sqrt(a_max * b))
        expected_s = s0 + max(0.0, expected_s)
        temp_a = pow(vel / vel0, delta)
        temp_b = pow(expected_s / s, 2)
        if vel<=0:
            a_max= 1
            temp_a = 0
            temp_b=0

        return a_max * (1 - temp_a - temp_b)

    def _purePursuit(self, prev_pt):
        """纯跟踪算法.[横向跟踪控制模块]
        输入:prev_pt——预瞄点
        输出:delta——前轮转角
        """
        lat_d = (prev_pt.y - self._ego_y) * math.cos(self._ego_yaw) - (prev_pt.x - self._ego_x) * math.sin(self._ego_yaw)  # 横向偏差
        Ld = math.hypot(prev_pt.y - self._ego_y, prev_pt.x - self._ego_x)  # 车与预瞄点距离
        delta = math.atan(2 * self.L * lat_d / (Ld**2))  # 前轮转角
        return delta

    def _findFrontVehicle(self, vehicle_info, path_waypoints, traj_future, observation):
        """ego车查找前车.
        输入:vehicle_info——背景车信息,path——自车轨迹
        输出:front_vehicle——前车信息
        """

        ######################制作投影##########################
        for key, vehicle in vehicle_info.items():
            if key == "ego":
                continue  # 如果是自车,跳过
            dist = math.hypot(vehicle["x"] - self._ego_x, vehicle["y"] - self._ego_y)  # 背景车与本车距离
            if dist > 50:
                continue  # 如果距离过大,跳过

            # 如果背景车与路径点距离足够小,投影到路径上
            # nearest_dist = self._tree.search_nn_dist([vehicle['x'],vehicle['y']])
            nearest_dist = self._tree.search_nn([vehicle["x"], vehicle["y"]])
            dist_temp = math.hypot(
                vehicle["x"] - nearest_dist[0].data.coords[0],
                vehicle["y"] - nearest_dist[0].data.coords[1],
            )  #
            # if nearest_dist<self._ego_shape['width']:
            if dist_temp < self._ego_shape["width"]:
                self._projectVehicle(vehicle, path_waypoints)
                continue

            # 预测0.5、1.0、1.5s后自车位置
            predict_ego = dict()
            for i in np.arange(0.5,self.time_car, 0.5):
                predict_ego_id = self._predictEgoVehicle(i, path_waypoints)  # 预测自车位置
                predict_ego_pt = path_waypoints[predict_ego_id]
                ego = dict()
                ego["x"] = predict_ego_pt[0]
                ego["y"] = predict_ego_pt[1]
                if predict_ego_id == len(path_waypoints) - 1:
                    yaw_vector = [
                        path_waypoints[predict_ego_id][0] - path_waypoints[predict_ego_id - 1][0],
                        path_waypoints[predict_ego_id][1] - path_waypoints[predict_ego_id - 1][1],
                    ]
                else:
                    yaw_vector = [
                        path_waypoints[predict_ego_id + 1][0] - path_waypoints[predict_ego_id][0],
                        path_waypoints[predict_ego_id + 1][1] - path_waypoints[predict_ego_id][1],
                    ]
                ego["yaw_rad"] = self._calYaw(yaw_vector[0], yaw_vector[1])
                ego["width"] = self._ego_shape["width"]
                ego["length"] = self._ego_shape["length"]
                predict_ego[i] = ego

            # 预测0.5、1.0、1.5s后背景车位置
            for i in np.arange(0.5, self.time_car, 0.5):
                if str(round(float(observation["test_setting"]["t"]) + i, 2)) in traj_future[key]:
                    time_number = round(float(observation["test_setting"]["t"]) + i, 2)  # ! xxx
                    predict_vehicle = traj_future[key][str(time_number)]
                    is_collision = self.__collisionDetect(
                        [
                            predict_ego[i]["x"],
                            predict_ego[i]["y"],
                            predict_ego[i]["yaw_rad"],
                            predict_ego[i]["width"],
                            predict_ego[i]["length"],
                        ],
                        [
                            predict_vehicle["x"],
                            predict_vehicle["y"],
                            predict_vehicle["yaw"],
                            vehicle["shape"]["width"],
                            vehicle["shape"]["length"],
                        ],
                    )
                    if is_collision == True:  # 如果相撞,投影到路径线上
                        self._projectVehicle(vehicle, path_waypoints)
                        time_str_list = []
                        vehicle_to_project = dict()
                        for j in np.arange(0.5, self.time_car, 0.5):
                            # time_str_list.append(round(float(observation['test_setting']['t'])+j,2))
                            time_str = str(round(float(observation["test_setting"]["t"]) + j, 2))
                            if time_str in traj_future[key]:
                                vehicle_to_project["x"] = traj_future[key][time_str]["x"]
                                vehicle_to_project["y"] = traj_future[key][time_str]["y"]
                                vehicle_to_project["yaw_rad"] = traj_future[key][time_str]["yaw"]
                                vehicle_to_project["v_mps"] = traj_future[key][time_str]["v"]
                                vehicle_to_project["acc_mpss"] = traj_future[key][time_str]["a"]
                                vehicle_to_project["shape"] = traj_future[key]["shape"]
                                self._projectVehicle(vehicle_to_project, path_waypoints)
                            else:
                                break
                        break
        ######################寻找最近前车##########################
        front_vehicle = None
        back_vehicle = None
        front_vehicle_match_id = 999999999
        for vehicle in self._projection_vehicle_S:
            # if vehicle["match_path_point_id"] <= self._nearest_pt_id:  # 在本车后方,跳过
            #     s = (
            #             math.hypot(self._ego_x - vehicle["x"], self._ego_y - vehicle["y"])
            #             - math.hypot(self._ego_shape["width"], self._ego_shape["length"]) / 2
            #             - math.hypot(vehicle["shape"]["width"], vehicle["shape"]["length"]) / 2
            #     )
            #     if s > 0:
            #         back_vehicle = vehicle
            if vehicle["match_path_point_id"] > self._nearest_pt_id:
                s = (
                    math.hypot(self._ego_x - vehicle["x"], self._ego_y - vehicle["y"])
                    - math.hypot(self._ego_shape["width"], self._ego_shape["length"]) / 2
                    - math.hypot(vehicle["shape"]["width"], vehicle["shape"]["length"]) / 2
                )
                if s > 0:
                    front_vehicle = vehicle
        return front_vehicle, back_vehicle

    def _predictEgoVehicle(self, t, path_waypoints):
        """预测T时间后自车位置(在待行驶路径上进行推算).
        输入:t——预测时间,path——自车轨迹
        输出:预测位置路径点下标
        """
        s = self._ego_v * t  # 行驶路程
        drive_s = 0.0
        waypoint = self._nearest_pt
        predict_pt_id = self._nearest_pt_id
        while drive_s < s:
            try:
                drive_s += math.hypot(
                    waypoint[0] - path_waypoints[predict_pt_id + 1][0],
                    waypoint[1] - path_waypoints[predict_pt_id + 1][1],
                )
                predict_pt_id += 1
                waypoint = path_waypoints[predict_pt_id]
            except:
                s -= self._ego_v * 0.5

        return predict_pt_id

    def _projectVehicle(self, vehicle, path_waypoints):
        """将车辆BOX投影到路径上.
        输入:vehicle——车辆,path——路径
        """
        nearest_p_id = int(self._tree.search_nn([vehicle["x"], vehicle["y"]])[0].data.data)

        project_vehicle = dict()  # 投影车辆
        project_vehicle["x"] = path_waypoints[nearest_p_id][0]
        project_vehicle["y"] = path_waypoints[nearest_p_id][1]
        project_vehicle["shape"] = dict()
        project_vehicle["shape"]["width"] = vehicle["shape"]["width"]
        project_vehicle["shape"]["length"] = vehicle["shape"]["length"]

        # project_vehicle['yawrate_rads'] = vehicle['yawrate_rads']
        project_vehicle["match_path_point_id"] = nearest_p_id

        #################计算投影轴#################
        if nearest_p_id == len(path_waypoints) - 1:
            projection_axis = [
                path_waypoints[nearest_p_id][0] - path_waypoints[nearest_p_id - 1][0],
                path_waypoints[nearest_p_id][1] - path_waypoints[nearest_p_id - 1][1],
            ]
        else:
            projection_axis = [
                path_waypoints[nearest_p_id + 1][0] - path_waypoints[nearest_p_id][0],
                path_waypoints[nearest_p_id + 1][1] - path_waypoints[nearest_p_id][1],
            ]
        #################计算车辆yaw角#################
        project_vehicle["yaw_rad"] = self._calYaw(projection_axis[0], projection_axis[1])
        #################投影速度#################
        v_vector = self._calProjectVector(
            [
                vehicle["v_mps"] * math.cos(vehicle["yaw_rad"]),
                vehicle["v_mps"] * math.sin(vehicle["yaw_rad"]),
            ],
            projection_axis,
        )
        project_vehicle["v_mps"] = math.sqrt(v_vector[0] ** 2 + v_vector[1] ** 2)
        #################投影加速度#################
        a_vector = self._calProjectVector(
            [
                vehicle["acc_mpss"] * math.cos(vehicle["yaw_rad"]),
                vehicle["acc_mpss"] * math.sin(vehicle["yaw_rad"]),
            ],
            projection_axis,
        )
        project_vehicle["acc_mpss"] = math.sqrt(a_vector[0] ** 2 + a_vector[1] ** 2)

        self._projection_vehicle_S.append(project_vehicle)

    def _calYaw(self, dx, dy):
        """计算yaw角.
        输入:dx,dy——向量
        输出:theta——yaw角
        """
        if dx==0:
            theta = 0
        else:
            theta = math.atan(dy / dx)
        if dx < 0:
            theta += 3.1415926
        if theta < 0:
            theta += 3.1415926 * 2
        return theta

    def _calProjectVector(self, vec1, vec2):
        """计算vec1车在vec2车上的投影
        输入:vec1——投影向量,vec2——投影轴
        输出:vec3——结果向量
        """
        dot = vec1[0] * vec2[0] + vec1[1] * vec2[1]  # 点乘结果
        dot = dot / (vec2[0] ** 2 + vec2[1] ** 2)

        # Convert vec2 to a numpy array for element-wise multiplication
        vec2_np = np.array(vec2)

        return vec2_np * dot

    def __collisionDetect(self, v1, v2):
        """两辆车碰撞检测."""
        # 计算车辆八个顶点
        x = v1[0]
        y = v1[1]
        yaw = v1[2]
        width = v1[3] + 0.2
        length = v1[4] + 0.2
        v1_p = [
            self.__vehicle2Global([x, y], [length / 2, width / 2], yaw),
            self.__vehicle2Global([x, y], [length / 2, -width / 2], yaw),
            self.__vehicle2Global([x, y], [-length / 2, -width / 2], yaw),
            self.__vehicle2Global([x, y], [-length / 2, width / 2], yaw),
        ]
        x = v2[0]
        y = v2[1]
        yaw = v2[2]
        width = v2[3] + 0.2
        length = v2[4] + 0.2
        v2_p = [
            self.__vehicle2Global([x, y], [length / 2, width / 2], yaw),
            self.__vehicle2Global([x, y], [length / 2, -width / 2], yaw),
            self.__vehicle2Global([x, y], [-length / 2, -width / 2], yaw),
            self.__vehicle2Global([x, y], [-length / 2, width / 2], yaw),
        ]
        # 投影1
        width = v1[3] + 0.2
        length = v1[4] + 0.2
        axis = [v1_p[0][0] - v1_p[1][0], v1_p[0][1] - v1_p[1][1]]
        vec = [v2_p[0][0] - v1_p[1][0], v2_p[0][1] - v1_p[1][1]]
        projection_dis1 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v2_p[1][0] - v1_p[1][0], v2_p[1][1] - v1_p[1][1]]
        projection_dis2 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v2_p[2][0] - v1_p[1][0], v2_p[2][1] - v1_p[1][1]]
        projection_dis3 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v2_p[3][0] - v1_p[1][0], v2_p[3][1] - v1_p[1][1]]
        projection_dis4 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        min_dis = min(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        max_dis = max(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        if min_dis > width or max_dis < 0:
            return False
        # 投影2
        axis = [v1_p[2][0] - v1_p[1][0], v1_p[2][1] - v1_p[1][1]]
        vec = [v2_p[0][0] - v1_p[1][0], v2_p[0][1] - v1_p[1][1]]
        projection_dis1 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v2_p[1][0] - v1_p[1][0], v2_p[1][1] - v1_p[1][1]]
        projection_dis2 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v2_p[2][0] - v1_p[1][0], v2_p[2][1] - v1_p[1][1]]
        projection_dis3 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v2_p[3][0] - v1_p[1][0], v2_p[3][1] - v1_p[1][1]]
        projection_dis4 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        min_dis = min(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        max_dis = max(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        if min_dis > length or max_dis < 0:
            return False
        # 投影3
        width = v2[3] + 0.2
        length = v2[4] + 0.2
        axis = [v2_p[2][0] - v2_p[1][0], v2_p[2][1] - v2_p[1][1]]
        vec = [v1_p[0][0] - v2_p[1][0], v1_p[0][1] - v2_p[1][1]]
        projection_dis1 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v1_p[1][0] - v2_p[1][0], v1_p[1][1] - v2_p[1][1]]
        projection_dis2 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v1_p[2][0] - v2_p[1][0], v1_p[2][1] - v2_p[1][1]]
        projection_dis3 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v1_p[3][0] - v2_p[1][0], v1_p[3][1] - v2_p[1][1]]
        projection_dis4 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        min_dis = min(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        max_dis = max(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        if min_dis > length or max_dis < 0:
            return False
        # 投影4
        axis = [v2_p[0][0] - v2_p[1][0], v2_p[0][1] - v2_p[1][1]]
        vec = [v1_p[0][0] - v2_p[1][0], v1_p[0][1] - v2_p[1][1]]
        projection_dis1 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v1_p[1][0] - v2_p[1][0], v1_p[1][1] - v2_p[1][1]]
        projection_dis2 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v1_p[2][0] - v2_p[1][0], v1_p[2][1] - v2_p[1][1]]
        projection_dis3 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        vec = [v1_p[3][0] - v2_p[1][0], v1_p[3][1] - v2_p[1][1]]
        projection_dis4 = (axis[0] * vec[0] + axis[1] * vec[1]) / math.hypot(axis[0], axis[1])
        min_dis = min(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        max_dis = max(projection_dis1, projection_dis2, projection_dis3, projection_dis4)
        if min_dis > width or max_dis < 0:
            return False
        return True

    def __vehicle2Global(self, v, point, yaw):
        x1 = point[0]
        y1 = point[1]
        x2 = x1 * math.cos(yaw) - y1 * math.sin(yaw) + v[0]
        y2 = x1 * math.sin(yaw) + y1 * math.cos(yaw) + v[1]
        return [x2, y2]
